#include <stdio.h>
#include <stdlib.h>
#include "zerdlib.h"
#include "Modules/uart/UART.h"

_FBS(BWRP_WRPROTECT_OFF & RBS_NO_BOOT_RAM & BSS_NO_BOOT_CODE)
_FSS(SWRP_WRPROTECT_OFF & RSS_NO_SEC_RAM & SSS_NO_SEC_CODE)
_FGS(GWRP_OFF & GSS_OFF)
_FOSCSEL(FNOSC_FRCPLL & IESO_OFF)
_FOSC(FCKSM_CSDCMD & IOL1WAY_OFF & OSCIOFNC_ON & POSCMD_NONE)
_FWDT(FWDTEN_OFF)
_FPOR(PWMPIN_ON & HPOL_ON & LPOL_ON & FPWRT_PWR1 & ALTI2C_OFF)
_FICD(JTAGEN_OFF & ICS_PGD1)

#define I2CLA_IDLE 0
#define I2CLA_START 1
#define I2CLA_TX 2
#define I2CLA_NACK 3
#define I2CLA_END 4
#define I2CLA_ADDR 5
#define I2CLA_RX 6

volatile U8 lastSendAction = I2CLA_IDLE;
volatile U16 sendDataCnt = 0;
volatile U8 * sendData;
volatile U16 sendDataSize = 0;
volatile U8 sendAddress = 0;

volatile U8 lastReceiveAction = I2CLA_IDLE;
volatile U8 rcvAddress;
volatile U8 rcvDataSize;
volatile U8 rcvData[15];
volatile U8 rcvDataCnt;

U8 dspicID = 0;

void send(U8 sendAddress_, U16 sendDataSize_, U8* sendData_);

void onMessage(I2C_Message msg) {
    UART1_printf("callback !!\r\n", dspicID);
}

int main(void) {
    U32 i;

    oscConfig();
    SetRAAsOutput();
    SetRA4AsInput();

    SetRAOutput(255);
    for (i = 0; i < 5000000; i++);

    UART1_init(0,1,38400,500,0,7,0,7);
    

    dspicID = GetRA4Input();
    UART1_printf("DsPIC n°%d\r\n", dspicID);


    I2C1_init();

    

    UART1_printf("I2C initialisé\r\n");

    U8 data[8] = {0,1,2,3,4};
    U16 cnt = 0;

    while(1) {
        for (i = 0; i < 5000000; i++);
        InvRAOutput();
        UART1_printf("\r\n");
        
        U16 size = cnt%4;
        data[0] = cnt++;
        //send(20, size, data);
    }
}

void (*I2C1_onMessage)(I2C_Message);
I2C_Message I2C1RX_buffer1, I2C1RX_buffer2;

I2C1_init(U8 maxMessageSize,void (*onMessage)(I2C_Message)) {
    I2C1RX_buffer1 = malloc(maxMessageSize + sizeof(struct I2C_Message_struct));
    I2C1RX_buffer2 = malloc(maxMessageSize + sizeof(struct I2C_Message_struct));
    I2C1RX_currentMessage = I2C1RX_buffer1;

    I2C1_onMessage = onMessage;

    _GCEN = 1; // interruption sur Général call activée

    I2C1MSK = 0x00FF; //masque de l'adresse
    I2C1BRG = 95; //400kHz si Fcy = 40MHz

    _MI2C1IP = 6;
    _MI2C1IE = 1;
    _MI2C1IF = 0;

    _SI2C1IP = 6;
    _SI2C1IF = 0;
    _SI2C1IE = 1;

    _I2CEN = 1;
}

void send(U8 sendAddress_, U16 sendDataSize_, U8* sendData_) {
    UART1_printf("send\r\n");

    sendDataCnt = 0;
    sendAddress = sendAddress_;
    sendData = sendData_;
    sendDataSize = sendDataSize_;

    lastSendAction = I2CLA_START;
    _SEN = 1;
}



void __attribute__((interrupt,no_auto_psv))_U1TXInterrupt(void)
{
    UART1_onU1TXInterrupt();
}


void __attribute__((interrupt,no_auto_psv)) _MI2C1Interrupt(void)
{
    if(_BCL) {
        lastSendAction = I2CLA_IDLE;
        UART1_printf("TX_BCL\r\n");
    } else if(lastSendAction == I2CLA_START) {
        UART1_printf("TX_ADDR : %d\r\n", sendAddress);
        I2C1TRN = sendAddress;
        lastSendAction = I2CLA_ADDR;
    } else if(lastSendAction == I2CLA_ADDR) {
        UART1_printf("TX_SIZE : %d\r\n", sendDataSize);
        I2C1TRN = sendDataSize;
        lastSendAction = I2CLA_TX;
    } else if(lastSendAction == I2CLA_TX) {
        if(_ACKSTAT != 0) {
            UART1_printf("TX_NACK1\r\n");
            lastSendAction = I2CLA_NACK;
            _PEN = 1;
        } else if(sendDataCnt == sendDataSize) {
            UART1_printf("TX_END1\r\n");
            lastSendAction = I2CLA_END;
            _PEN = 1;
        } else {
            UART1_printf("TX_DATA : %d  (%d/%d)\r\n", sendData[sendDataCnt], sendDataCnt, sendDataSize);
            lastSendAction = I2CLA_TX;
            I2C1TRN = sendData[sendDataCnt++];
        }
    } else if(lastSendAction == I2CLA_NACK) {
        UART1_printf("TX_NACK2\r\n");
        sendDataCnt = 0;
        lastSendAction = I2CLA_START;
        _SEN=1;
    } else if(lastSendAction == I2CLA_END) {
        UART1_printf("TX_END2\r\n");
        // dépiler le message
    } else {
        UART1_printf("TX_BUG\r\n");
        // Théoriquement impossible
    }

    _MI2C1IF = 0;
}


enum I2C1RX_STATE { I2C1RX_STATE_IDLE, I2C1RX_STATE_RCV };

struct I2C_Message_struct {
    union {
        U8 adresse8;
        struct {
            U8 adresse :7;
            U8 RW : 1;
        };
    };
    
    U8 size;
    U8 data[];
};
typedef  struct I2C_Message_struct* I2C_Message;

volatile I2C_Message I2C1RX_currentMessage;
volatile S16 I2C1RX_data_cnt = -1;

void __attribute__((interrupt,no_auto_psv)) _SI2C1Interrupt(void)
{
    if(I2C1STATbits.D_A == 0) {
        // l'octet reçu est une adresse
        if(I2C1RX_data_cnt != -1) {
            // Restart alors qu'on était en cours de message : abandon de message en cours
            UART1_printf("RX_NOENDMESSAGE\r\n");
            I2C1RX_data_cnt = -1;
        }
        
        // le module est prêt à recevoir un message
        I2C1RX_currentMessage->adresse8 = I2C1RCV;
        UART1_printf("RX_ADDR : %d (%c)\r\n", I2C1RX_currentMessage.adresse, I2C1RX_currentMessage.RW?'R':'W');
    } else {
        // octet de donnée reçu (du point de vue du protocole I2C, donc contient la taille de nos messages)
        if(I2C1RX_data_cnt == -1) {
            // tout premier octet : la taille du message
            I2C1RX_currentMessage->size = I2C1RCV;
            I2C1RX_data_cnt++;
            UART1_printf("RX_SIZE : %d\r\n", I2C1RX_currentMessage.size);
        } else {
            //donnée standard
            I2C1RX_currentMessage->data[I2C1RX_data_cnt++];
            UART1_printf("RX_SIZE : %d\r\n", I2C1RX_currentMessage.data[I2C1RX_data_cnt]);
        }

        if(I2C1RX_data_cnt == I2C1RX_currentMessage->size) {
            // le message est fini
            UART1_printf("RX_END\r\n");
            I2C1_onMessage(I2C1RX_currentMessage);
            I2C1RX_data_cnt = -1;
        }
    }

    _SI2C1IF = 0;
}
